#ifndef FRENET_OPTIMAL_TRAJECTORY_H
#define FRENET_OPTIMAL_TRAJECTORY_H

#include "FrenetPath.h"
#include <vector>
#include <cmath>
#include <queue>
#include <chrono>
#include <complex>
#include <iomanip>
#include <ros/ros.h>
#include <algorithm>
#include <glog/logging.h>
#include "paths_struct.h"
#include "FrenetPath.h"
#include "QuinticPolynomial.h"
#include "QuarticPolynomial.h"
#include "PathManager.h"
#include "cartesian_to_frenet.h"
#include "paths_routing_parameters.h"
#include "CubicSpline2D.h"

struct CostComparator_for_Frenet : public std::binary_function<const FrenetPath*, const FrenetPath*, bool>
{
    bool operator()(const FrenetPath* left, const FrenetPath* right) const
    {
        return left->W > right->W;
    }
};

class FrenetOptimalTrajectory
{
public:
    FrenetOptimalTrajectory();
    ~FrenetOptimalTrajectory();
    void calc_frenet_paths(FrenetInitialConditions* fot_ic, FrenetHyperparameters* fot_hp,
                                                std::priority_queue<FrenetPath*, std::vector<FrenetPath*>, CostComparator_for_Frenet>& frenet_path, 
                                                std::vector<std::pair<double, double>>& global_points, std::vector<double>& accumulated_s, int& vehicle_index, 
                                                const std::unique_ptr<CubicSpline2D>& Cub_spline_2D, const std::vector<std::pair<double, double>>& s_d_point);
    bool to_global_path(FrenetPath &fts, std::vector<std::pair<double, double>>& global_points, 
                                                std::vector<double>& accumulated_s, int& vehicle_index, const std::unique_ptr<CubicSpline2D>& Cub_spline_2D);
    bool is_valid_path(FrenetPath &fts, const std::vector<std::pair<double, double>>& s_d_point);
private:

    // PathManager pathM;
    CartesianToFrenet frenetToCar;
    Paths_Routing_Parameters path_par;

};

#endif  // FRENET_OPTIMAL_TRAJECTORY_H